#include "navigate_to_sorting_area.h"

NavigateToSortingArea::NavigateToSortingArea(const std::string& name, const BT::NodeConfig& config,
                                             rclcpp::Node* node)
    : BT::StatefulActionNode(name, config), node_(node), navigation_complete_(false)
{
}

BT::PortsList NavigateToSortingArea::providedPorts() { return {}; }

BT::NodeStatus NavigateToSortingArea::onStart()
{
    RCLCPP_INFO(node_->get_logger(), "开始导航到分拣区");
    navigation_complete_ = false;

    // 模拟导航过程
    timer_ = node_->create_wall_timer(std::chrono::seconds(1),
                                      [this]()
                                      {
                                          RCLCPP_INFO(node_->get_logger(), "到达分拣区");
                                          navigation_complete_ = true;
                                          timer_->cancel();
                                      });

    return BT::NodeStatus::RUNNING;
}

BT::NodeStatus NavigateToSortingArea::onRunning()
{
    return navigation_complete_ ? BT::NodeStatus::SUCCESS : BT::NodeStatus::RUNNING;
}

void NavigateToSortingArea::onHalted() { RCLCPP_WARN(node_->get_logger(), "导航任务被中断"); }